#include "vectorMath.h"
#include <cassert>

namespace moremath{
Mat3d getCrossMat(Vec3d v){
	Mat3d m(vl_0);
	m[0][1]=-v[2];
	m[1][0]=v[2];
	m[0][2]=v[1];
	m[2][0]=-v[1];
	m[1][2]=-v[0];
	m[2][1]=v[0];
	return m;
}

void getDerivVecHat(Vec3d v, Mat3d *m){
	double v_mag = len(v);
	Vec3d vhat = norm(v);
	*m = (Mat3d(vl_1) - oprod(vhat,vhat))/v_mag;
}

// returns a 1x3 vec m
void getDerivVecMag(Vec3d v, Vec3d *m){
	*m = v/len(v);
}

// returns a 1x3 matrix
void getDerivVecMag(Vec3d v, Matd *m){
	*m = Matd(1,3, vl_0);
	Vec3d der;
	getDerivVecMag(v, &der);
	for(int i=0; i<3; i++) (*m)[0][i] = der[i];
}

// assume numelts in _v is 1 less than _m
Vecd xformHom(Matd _m, Vecd _v){
	assert(_m.Cols()==_v.Elts()+1);
	Vecd newv(_v.Elts()+1, vl_0);
	for(int i=0; i<_v.Elts(); i++) newv[i] = _v[i];
	newv[newv.Elts()-1] = 1.0;
	Vecd res = _m*newv;
	Vecd newres(res.Elts()-1, vl_0);
	for(int i=0; i<newres.Elts(); i++) newres[i] = res[i];
	//newres/=res[res.Elts()-1];
	return newres;
}

}

Vecd sqrt(Vecd _v){
	for(int i=0; i<_v.Elts(); i++){
		_v[i] = sqrt(_v[i]);
	}
	return _v;
}
